#!/usr/bin/env python
'''This is a demo on how to use Gnuplot for potential fields.  We've
intentionally avoided "giving it all away."
'''
from __future__ import division
from itertools import cycle
from pfagent import PField
from bzrc import BZRC
from searches import *
import optparse, math
WORLDSIZE = 800

if __name__ == '__main__':


	# Process CLI arguments.
	parser = optparse.OptionParser(usage = "%prog [options]",
									   version = "%prog 0.1")
	parser.add_option("-f", "--fileName", type="string",dest="fileName",
				metavar="fileName",default='lines.gpi',
				help="hostname for the server")
	parser.add_option("-o", "--host", type="string",dest="host",
				metavar="HOST",default='localhost',
				help="hostname for the server")
	parser.add_option("-p", "--port", type="int",dest="port",
				metavar="PORT",default=3907,
				help="port number for the server")
	parser.add_option("-i", "--max_iterations", type="int",dest="maxIterations",
				metavar="maxIterations",default=1000,
				help="maximum number of search iterations")
	parser.add_option("-l", "--fullSearch", action="store_true", dest="fullSearch", default=False)

	(options, args) = parser.parse_args()

	bzrc = BZRC(options.host, int(options.port))
	fileName = options.fileName

	nodes = bzrc.get_occgrid(0)
	mytank = bzrc.get_mytanks()[0]
	flags = bzrc.get_flags()
	flag = None
	for f in flags:
		if f.color == 'green':
			flag = f
			break
		
	constants = bzrc.get_constants()
	othertanks = bzrc.get_othertanks()
	#nodes, goal, penalties = parse_grid(othertanks, int(constants['worldsize']), bzrc) 
	searcher = AStar(bzrc, int(mytank.x), int(mytank.y), WORLDSIZE) #bzrc, curX, curY, size

	########################################################################
	# Constants

	# Size of the world (one of the "constants" in bzflag):
	WORLDSIZE = 800
	# Animation parameters:
	ANIMATION_MIN = 0
	ANIMATION_MAX = 500
	ANIMATION_FRAMES = 25

	OBSTACLES = bzrc.get_obstacles()


########################################################################
# Helper Functions

def gpi_point(x, y, vec_x, vec_y):
	'''Create the centered gpi data point (4-tuple) for a position and
	vector.  The vectors are expected to be less than 1 in magnitude,
	and larger values will be scaled down.'''
	r = (vec_x ** 2 + vec_y ** 2) ** 0.5
	if r > 1:
		vec_x /= r
		vec_y /= r
	return (x - vec_x * VEC_LEN / 2, y - vec_y * VEC_LEN / 2,
			vec_x * VEC_LEN, vec_y * VEC_LEN)

def gnuplot_header(minimum, maximum):
	'''Return a string that has all of the gnuplot sets and unsets.'''
	s = ''
	s += 'set xrange [%s: %s]\n' % (minimum, maximum)
	s += 'set yrange [%s: %s]\n' % (minimum, maximum)
	# The key is just clutter.  Get rid of it:
	s += 'unset key\n'
	# Make sure the figure is square since the world is square:
	s += 'set size square\n'
	# Add a pretty title (optional):
	s += "set title 'Search'\n"
	return s

def draw_line(p1, p2, color=3):
	"""Return a string to tell Gnuplot to draw a line from point p1 to
	point p2 in the form of a set command."""
	x1, y1 = p1
	x2, y2 = p2
	s = 'set arrow from %s, %s to %s, %s nohead lt '% (x1, y1, x2, y2) + str(color) + '\n' 
	return s

def draw_obstacles(obstacles):
	'''Return a string which tells Gnuplot to draw all of the obstacles.'''
	s = 'unset arrow\n'

	for obs in obstacles:
		last_point = obs[0]
		for cur_point in obs[1:]:
			s += draw_line(last_point, cur_point)
			last_point = cur_point
		s += draw_line(last_point, obs[0])
	return s
   

def start_multiplot():
	return "set multiplot\n"

# creates a string that draws a line from the start to the end of a list of nodes
def plot_nodes(nodes, color):
	print "Beginning plot_nodes"
	lastNode = nodes[0]
	s = "set pointsize 1\nplot '-' with lines lt "+ str(color) + "\n"
	print "Nodes: ", nodes
	for node in nodes[1:]:
		print "LastNode: ", lastNode
		x1, y1 = lastNode
		x2, y2 = node
		s += '%s %s %s %s\n' % (x1, y1, x2, y2)
		lastNode = node
	print "Done with plot_nodes"
	s += 'e\n'
	return s
	
	# creates a string that draws a point for each node
def plot_nodesp(nodes, color):
	print "Beginning plot_nodes"
	s = "set pointsize 1\nplot '-' with points lt "+ str(color) + "\n"
	for node in nodes[1:]:
		x2, y2 = node
		s += '%s %s\n' % (x2, y2)
	print "Done with plot_nodes"
	s += 'e\n'
	return s

# creates a string drawing lines from the last node to all of the nodes added to the frontier	
def plot_frontier():
	s = ''
	nodes = searcher.step(100)
	lastNode = searcher.lastNode
	for node in nodes:
		s += draw_line((node[0], node[1]), (lastNode[0], lastNode[1]), 2)
	return s

def pause(delay):
	return "plot '-' with lines\n0 0 0 0\ne\npause %f\n" % (delay)

########################################################################
# Plot the potential fields to a file
"""
outfile = open(fileName, 'w')
print >>outfile, gnuplot_header(-WORLDSIZE / 2, WORLDSIZE / 2)
print >>outfile, draw_obstacles(OBSTACLES)
if(options.fullSearch):
	finalPath, allSearched = searcher.search()
	print >>outfile, start_multiplot()
	print >>outfile, plot_nodes(allSearched, 1)
	print >>outfile, plot_nodes(finalPath, 2)
else:
	for i in range(options.maxIterations):
		print >>outfile, plot_frontier()
"""
########################################################################
# Animate a changing field, if the Python Gnuplot library is present

if __name__ == '__main__':
	try:
		from Gnuplot import GnuplotProcess
	except ImportError:
		print "Sorry.  You don't have the Gnuplot module installed."
		import sys
		sys.exit(-1)

	gp = GnuplotProcess(persist=1)
	gp.write(gnuplot_header(-WORLDSIZE / 2, WORLDSIZE / 2))
	gp.write(draw_obstacles(OBSTACLES))
	searcher.clear()
	if(options.fullSearch):
		finalPath, allSearched = searcher.search()
		gp.write(start_multiplot())
		gp.write(plot_nodesp(allSearched, 1))
		gp.write(plot_nodes(finalPath, 2))
		gp.flush()
	else:
		gp.write(start_multiplot())
		for i in range(options.maxIterations):
			gp.write(plot_frontier())
			gp.write(pause(1 / ANIMATION_FRAMES))
			gp.flush()
			time.sleep(1 / ANIMATION_FRAMES)

# vim: et sw=4 sts=4
